/*
 * TJunctions_Slave.c
 *
 * Created: 6/3/2013 9:32:11 PM
 *  Author: Crossfire
 */


#include "TJunctions_Slave.h"

int main(void)
{
	//Prescale 64
	TCCR1B |= 0b00000011;
	_delay_ms(500);
	TCNT1 = 0;

	_delay_1 = 0;
	_delay_2 = 0;
	_state_1 = 0;
	_state_2 = 0;
	TWI_init_slave(); // Function to initilaize slave
	DDRA = 0xC0;
	DDRB = 0x0F;
	DDRC = 0xFC;
	DDRD = 0xFC;
	while(1)
	{
		while((TWSR & 0xF8)!= 0x60)  // Loop till correct acknoledgement have been received
		{
			// Get acknowlegement, Enable TWI, Clear TWI interrupt flag
			TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);
			while (!(TWCR & (1<<TWINT)))  // Wait for TWINT flag
			{
				if (TCNT1 > 7812)
				{
					if (_delay_1>0)
					{
						_delay_1--;
					}
					if (_delay_2>0)
					{
						_delay_2--;
					}
					TCNT1=0;
				}
				//dostuff while waiting for I2C communication
				set_state(1, &_state_1, &_delay_1);
				set_state(2, &_state_2, &_delay_2);
				_byteA = 0;
                _byteB = 0;
                _byteC = 0;
                _byteD = 0;
				set_stoplichten(_mode, 1);
				set_stoplichten(_mode, 2);
				PORTA = _byteA;
				PORTB = _byteB;
				PORTC = _byteC;
				PORTD = _byteD;
			}
		}
		parse_data_tsplitsing();
	}
}

void parse_data_tsplitsing()
{
	unsigned char data = 0;
	unsigned char save = 0;
	int shift = 6;
	int amount_read = 0;
	TWI_match_read_slave();
	TWI_read_slave();
	amount_read++;
	_mode = (recv_data & 1);
	PORTB = _mode;
	PORTB = ~0;
	TWI_match_read_slave();
	TWI_read_slave();
	amount_read++;
	data = recv_data;
	//
	for(int i = 0; i < 7; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			save = ((data >> shift) & 3);
			_tjunction[i][j] = save;
			shift -=2;
			if(shift == -2)
			{
				shift = 6;
				TWI_match_read_slave();
				TWI_read_slave();
				data = recv_data;
				amount_read++;
			}
		}
	}
	while(amount_read < 57)
	{
		TWI_match_read_slave();
		TWI_read_slave();
		amount_read++;
		PORTB = amount_read;
	}
	PORTB = 1;
}

// Function to initilaize slave
void TWI_init_slave(void)
{
	// Fill slave address to TWAR
	TWAR=TJUNCTION;
}

void TWI_read_slave(void)
{
	// Clear TWI interrupt flag,Get acknowlegement, Enable TWI
	TWCR= (1<<TWINT)|(1<<TWEA)|(1<<TWEN);
	// Wait for TWINT flag
	while (!(TWCR & (1<<TWINT)));
	// Wait for acknowledgement
	while((TWSR & 0xF8)!=0x80);
	// Get value from TWDR
	recv_data=TWDR;
	// send the receive value on PORTB
	//PORTB=recv_data;

}

//Function to match the slave address and slave dirction bit(read)
void TWI_match_read_slave(void)
{
	//Loop till correct acknoledgement have been received
	while((TWSR & 0xF8)!= 0x60)
	{
		//Get acknowlegement, Enable TWI, Clear TWI interrupt flag
		TWCR=(1<<TWEA)|(1<<TWEN)|(1<<TWINT);

		//Wait for TWINT flag
		while (!(TWCR & (1<<TWINT)));
	}
}

//Functie om verkeerslichten op de juiste modus te zetten, geen delay
void set_stoplichten(int mode, int splitsing)
{
	//Tijdelijke array om data in op te slaan
	uint8_t Temp_Array[7][3];

	//Als mode gelijk is aan 1, dan wordt de statemachine van de web applicatie afkomt gebruikt
	if(mode == 1)
	{
		for(int i = 0; i < 7; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				//Statemachine webapplicatie wordt in de tijdelijke array gezet
				Temp_Array[i][j] = _tjunction[i][j];
			}
		}
	}
	//anders als mode gelijk is aan 0, dan wordt de default statemachine gebruikt
	else if(mode == 0)
	{
		for(int i = 0; i < 7; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				//Default statemachine wordt in de tijdelijke array gezet
				Temp_Array[i][j] = _default[i][j];
			}
		}
	}
	if(splitsing == 1)
	{
		//Als de tijdelijke array state x is en verkeerslicht 1 is op rood
		if (Temp_Array[_state_1][0] == 1)
		{
			_byteB |= 0b00000010;
		}
		//Als de tijdelijke array state x is en verkeerslicht 1 is op oranje
		if (Temp_Array[_state_1][0] == 2)
		{
			_byteB |= 0b00000100;
		}
		//Als de tijdelijke array state x is en verkeerslicht 1 is op groen
		if (Temp_Array[_state_1][0] == 3)
		{
			_byteB |= 0b00001000;
		}

		//------------------------------------------------------------------------------
		//Als de tijdelijke array state x is en verkeerslicht 2 is op rood
		if (Temp_Array[_state_1][1] == 1)
		{
			_byteA |= 0b10000000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 2 is op oranje
		if (Temp_Array[_state_1][1] == 2)
		{
			_byteA |= 0b01000000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 2 is op groen
		if (Temp_Array[_state_1][1] == 3)
		{
			_byteB |= 0b00000001;
		}

		//-------------------------------------------------------------------------------
		//Als de tijdelijke array state x is en verkeerslicht 3 is op rood
		if (Temp_Array[_state_1][2] == 1)
		{
			_byteC |= 0b00010000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 3 is op oranje
		if (Temp_Array[_state_1][2] == 2)
		{
			_byteC |= 0b00001000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 3 is op groen
		if (Temp_Array[_state_1][2] == 3)
		{
			_byteC |= 0b00000100;
		}
	}
	else
	{		//Als de tijdelijke array state x is en verkeerslicht 1 is op rood
		if (Temp_Array[_state_2][0] == 1)
		{
			_byteC |= 0b10000000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 1 is op oranje
		if (Temp_Array[_state_2][0] == 2)
		{
			_byteC |= 0b01000000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 1 is op groen
		if (Temp_Array[_state_2][0] == 3)
		{
			_byteC |= 0b00100000;
		}

		//------------------------------------------------------------------------------
		//Als de tijdelijke array state x is en verkeerslicht 2 is op rood
		if (Temp_Array[_state_2][1] == 1)
		{
			_byteD |= 0b00100000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 2 is op oranje
		if (Temp_Array[_state_2][1] == 2)
		{
			_byteD |= 0b01000000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 2 is op groen
		if (Temp_Array[_state_2][1] == 3)
		{
			_byteD |= 0b10000000;
		}

		//-------------------------------------------------------------------------------
		//Als de tijdelijke array state x is en verkeerslicht 3 is op rood
		if (Temp_Array[_state_2][2] == 1)
		{
			_byteD |= 0b00000100;
		}
		//Als de tijdelijke array state x is en verkeerslicht 3 is op oranje
		if (Temp_Array[_state_2][2] == 2)
		{
			_byteD |= 0b00001000;
		}
		//Als de tijdelijke array state x is en verkeerslicht 3 is op groen
		if (Temp_Array[_state_2][2] == 3)
		{
			_byteD |= 0b00010000;
		}
	}
	
}

//Functie om status sensoren te krijgen
int get_sensors()
{
	int sensors = PINA & 0x3F;
	//PORTB = sensors;
	return sensors;
	//needs work, Don't yet know what pins to use, now pinA0 till pinA5 are used.
}

//Functie om te bapelen welke state je in zit
void set_state(int splitsing, int * state, int * delay)
{
	int sensor = get_sensors();
	if(splitsing == 2)
        sensor = sensor >> 3;
	if(*delay == 0)
	{
		switch(*state)
		{
			case 0:
			{
				//Als sensor & 1 gelijk is aan 1, dan wordt het state 6
				if((sensor & 1) == 0)
				{
					*state = 5;
					*delay = 8;
				}
				//Als sensor & 2 gelijk is aan 2, dan wordt het state 4
				else if((sensor & 2) == 0)
				{
					*state = 3;
					*delay = 8;
				}
				//Als sensor & 4 gelijk is aan 4, dan wordt het state 2
				else if((sensor & 4) == 0)
				{
					*state = 1;
					*delay = 8;
				}
				//Anders wordt het state 1
				else
				{
					*state = 0;
				}
			}
			break;
			case 1:
			{
				//Als sensor & 4 gelijk is aan 0, dan wordt het state 3
				if((sensor & 4) != 0)
				{
					*state = 2;
					*delay = 4;
				}
			}
			break;
			case 2:
			{
				//Als sensor & 1 gelijk is aan 1, dan wordt het state 6
				if((sensor & 1) == 0)
				{
					*state = 5;
					*delay = 8;
				}
				//Als sensor & 2 gelijk is aan 2, dan wordt het state 4
				else if((sensor & 2) == 0)
				{
					*state = 3;
					*delay = 8;
				}
				//Als sensor & 4 gelijk is aan 4, dan wordt het state 2
				else if((sensor & 4) == 0)
				{
					*state = 1;
					*delay = 8;
				}
				//Anders wordt het state 1
				else
				{
					*state = 0;
				}
			}
			break;
			case 3:
			{
				//Als sensor & 2 gelijk is aan 0, dan wordt het state 5
				if((sensor & 2) != 0)
				{
					*state = 4;
					*delay = 4;
				}
			}
			break;
			case 4:
			{
				//Als sensor & 1 gelijk is aan 1, dan wordt het state 6
				if((sensor & 1) == 0)
				{
					*state = 5;
					*delay = 8;
				}
				//Als sensor & 2 gelijk is aan 2, dan wordt het state 4
				else if((sensor & 2) == 0)
				{
					*state = 3;
					*delay = 8;
				}
				//Als sensor & 4 gelijk is aan 4, dan wordt het state 2
				else if((sensor & 4) == 0)
				{
					*state = 1;
					*delay = 8;
				}
				//Anders wordt het state 1
				else
				{
					*state = 0;
				}
			}
			break;
			case 5:
			{
				//Als sensor & 1 gelijk is aan 0, dan wordt het state 1
				if((sensor & 1) != 0)
				{
					*state = 6;
					*delay = 4;
				}
			}
			break;
			case 6:
				//Als sensor & 1 gelijk is aan 1, dan wordt het state 6
				if((sensor & 1) == 0)
				{
					*state = 5;
					*delay = 8;
				}
				//Als sensor & 2 gelijk is aan 2, dan wordt het state 4
				else if((sensor & 2) == 0)
				{
					*state = 3;
					*delay = 8;
				}
				//Als sensor & 4 gelijk is aan 4, dan wordt het state 2
				else if((sensor & 4) == 0)
				{
					*state = 1;
					*delay = 8;
				}
				//Anders wordt het state 1
				else
				{
					*state = 0;
				}
			break;
		}
	}
}
